
#include "Constraint.h"
#include "World.h"

namespace p2 {

Constraint::~Constraint()
{
}

void Constraint::preStep(real64 dT)
{
	UNUSED(dT);
}

void Constraint::computeFutureAnchorPositions(real64 dT)
{
    /*
     * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
     * 'First an impulse P must be found to satisfy the point position
     * constraint. The impulse P will be applied at point A to the
     * first rigid body and the impulse -P at point B to the second
     * rigid body. These impulses must eliminate the distance
     * between the two points at time t0 + h.'
     * => work future point positions at t0 + h
     */
	Entity const* e0 = _anchor0.getEntity();
	Entity const* e1 = _anchor1.getEntity();

	if ( e0 ) {
    	_xAth = e0->getX() + (e0->getV()+_dv[0].getV()) * (real)dT;
    	Matrix2<real> rot(e0->getA() + (e0->getW()+_dv[0].getW()) * (real)dT);
        _rAth = _anchor0.getPoint() * rot;
    } else {
    	_xAth = _anchor0.getPoint();
        _rAth = Vector2<real>::ZERO;
    }
    if ( e1 ) {
        _xBth = e1->getX() + (e1->getV()+_dv[1].getV()) * (real)dT;
    	Matrix2<real> rot(e1->getA() + (e1->getW()+_dv[1].getW()) * (real)dT);
        _rBth = _anchor1.getPoint() * rot;
    } else {
    	_xBth = _anchor1.getPoint();
        _rBth = Vector2<real>::ZERO;
    }
	_uth = _xBth + _rBth - _xAth - _rAth;
	_duth = _uth.length();
}

void Constraint::updateVelocityChange()
{
	Entity* e0 = _anchor0.getEntity();
	Entity* e1 = _anchor1.getEntity();
	if ( e0 ) {
		e0->incV(_dv[0].getV());
		e0->incW(_dv[0].getW());
    }
    if ( e1 ) {
    	e1->incV(_dv[1].getV());
    	e1->incW(_dv[1].getW());
    }
	_dv[0].reset();
	_dv[1].reset();
}

}
